
require 'thread'

class Tester

  COMPONENT_NAME = :tester

  COMMAND_LOOP_INTERVAL = 0.1   # [second]

  def initialize
  end

  def init_as_component(component_mananger)
    @component_mananger = component_mananger
    @servo = @component_mananger.component(:servo)

    @command_queue = Queue.new

    Thread.new do
      begin
        command_loop
      rescue
        STDERR.puts "#{$!}"
      end
    end
  end

  def circle(radius, feed_rate, num_vertices)
    @servo.set_feed_rate(feed_rate)
    
    (num_vertices + 1).times do |i|
      r = (i.to_f / num_vertices) * 2 * Math::PI
      pos = [Math.cos(r) * radius, Math.sin(r) * radius, 0]
      p pos
      @command_queue.push(pos)
    end
  end

  private
  def command_loop
    while true
      if @servo.in_position? then
        pos = @command_queue.pop
        @servo.set_command_pos(pos)
      end
      sleep COMMAND_LOOP_INTERVAL
    end
  end
end
